/*
  Color test
    hardware: M5Stack

  please install the Adfruit TCS34725 library first ...
*/

#include <M5StickCPlus.h>
#include <Wire.h>
#include "Adafruit_TCS34725.h"

// set to false if using a common cathode LED
#define commonAnode true

// our RGB -> eye-recognized gamma color
byte gammatable[256];

static uint16_t color16(uint16_t r, uint16_t g, uint16_t b) {
    uint16_t _color;
    _color = (uint16_t)(r & 0xF8) << 8;
    _color |= (uint16_t)(g & 0xFC) << 3;
    _color |= (uint16_t)(b & 0xF8) >> 3;
    return _color;
}

Adafruit_TCS34725 tcs =
    Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_50MS, TCS34725_GAIN_4X);

void setup() {
    delay(100);

    M5.begin(true, false, false);
    Serial.begin(115200);
    Serial.println("Color View Test!");

    if (tcs.begin()) {
        Serial.println("Found sensor");
    } else {
        Serial.println("No TCS34725 found ... check your connections");
        while (1)
            ;  // halt!
    }
    tcs.setIntegrationTime(TCS34725_INTEGRATIONTIME_154MS);
    tcs.setGain(TCS34725_GAIN_4X);
}

void loop() {
    uint16_t clear, red, green, blue;

    delay(60);  // takes 50ms to read

    tcs.getRawData(&red, &green, &blue, &clear);

    Serial.print("C:\t");
    Serial.print(clear);
    Serial.print("\tR:\t");
    Serial.print(red);
    Serial.print("\tG:\t");
    Serial.print(green);
    Serial.print("\tB:\t");
    Serial.print(blue);

    // Figure out some basic hex code for visualization
    uint32_t sum = clear;
    float r, g, b;
    r = red;
    r /= sum;
    g = green;
    g /= sum;
    b = blue;
    b /= sum;
    r *= 256;
    g *= 256;
    b *= 256;
    Serial.print("\t");
    Serial.print((int)r, HEX);
    Serial.print((int)g, HEX);
    Serial.print((int)b, HEX);
    Serial.println();
    uint16_t _color = color16((int)r, (int)g, (int)b);
    M5.Lcd.fillScreen(_color);
}
